clc;clear all;close all;

%System Data

A10=[1.4 0.1;-0.4 0.2];A20=[1.1 0.2;0.3 0.4];
B10=[0 0.01;0.02 0];B20=[0 0.01;0.02 0];
D10=[-0.7;1];D20=[0.8;-0.5];
C2=[3 0.7];C1=C2;
E1=0.1*eye(2);F1=0.1*eye(2);G1=[0.1;0];
E2=0.2*eye(2);F2=0.2*eye(2);G2=[0.2;0];
e1=0.08;gama=1;
H1=[0.01 0;0 0.01];H2=[0.01 0;0 0.01];
r=1;

n=2;m=1;l=1;
eps=0.08;dM=4;
R1=eye(l);R2=eye(m);
Jsb=0;

%Controller Design

tic
m1=1+eps;   m1=sqrt(m1);
m2=(2*r+1)*(1+eps^-1);   m2=sqrt(m2);
E1bar=e1*(E1); F1bar=e1*(F1); G1bar=e1*(G1);
E2bar=e1*(E2); F2bar=e1*(F2); G2bar=e1*(G2);
alpha=1;

[U1,C10,V1] = svd(C1);
[U2,C20,V2] = svd(C2);

Vp1=V1(1:n,1:l);Vpp1=V1(1:n,(l+1):(n-l+1));
Vp2=V2(1:n,1:l);Vpp2=V2(1:n,(l+1):(n-l+1));

C10=C10(1:l,1:l);
C20=C20(1:l,1:l);

setlmis([]) ;
%X1 = lmivar(1,[n 1]);X2 = lmivar(1,[n 1]);
X11 = lmivar(1,[l 1]);X12 = lmivar(1,[l 1]);
X21 = lmivar(1,[n-l 1]);X22 = lmivar(1,[n-l 1]);
T1 = lmivar(1,[n 1]);T2 = lmivar(1,[n 1]);
Z1 = lmivar(2,[m l]);Z2 = lmivar(2,[m l]);

%lmiterm([-1 1 1 X1],1,1)
%lmiterm([-2 1 1 X2],1,1)
lmiterm([-1 1 1 X11],1,1)
lmiterm([-2 1 1 X12],1,1)
lmiterm([-3 1 1 X21],1,1)
lmiterm([-4 1 1 X22],1,1)
lmiterm([-5 1 1 T1],1,1)
lmiterm([-6 1 1 T2],1,1)

lmiterm([7 1 1 X11],-Vp1,Vp1')
lmiterm([7 1 1 X21],-Vpp1,Vpp1')
lmiterm([7 1 1 T1],dM,1)
lmiterm([7 1 3 X11],m1*Vp1,Vp1'*A10')
lmiterm([7 1 3 X21],m1*Vpp1,Vpp1'*A10')
lmiterm([7 1 3 -Z1],m1*C1',D10')
lmiterm([7 1 4 X11],m2*Vp1,Vp1'*E1bar')
lmiterm([7 1 4 X21],m2*Vpp1,Vpp1'*E1bar')
lmiterm([7 1 4 -Z1],m2*C1',G1bar')
lmiterm([7 1 6 X11],gama*m2*Vp1,Vp1'*H1')
lmiterm([7 1 6 X21],gama*m2*Vpp1,Vpp1'*H1')
lmiterm([7 2 2 T1],-1,1)
lmiterm([7 2 3 X11],m1*Vp1,Vp1'*B10')
lmiterm([7 2 3 X21],m1*Vpp1,Vpp1'*B10')
lmiterm([7 2 5 X11],m2*Vp1,Vp1'*F1bar')
lmiterm([7 2 5 X21],m2*Vpp1,Vpp1'*F1bar')
lmiterm([7 3 3 X12],-Vp2,Vp2')
lmiterm([7 3 3 X22],-Vpp2,Vpp2')
lmiterm([7 4 4 X12],-Vp2,Vp2')
lmiterm([7 4 4 X22],-Vpp2,Vpp2')
lmiterm([7 5 5 X12],-Vp2,Vp2')
lmiterm([7 5 5 X22],-Vpp2,Vpp2')
lmiterm([7 6 6 0],-1)

lmiterm([8 1 1 X12],-Vp2,Vp2')
lmiterm([8 1 1 X22],-Vpp2,Vpp2')
lmiterm([8 1 1 T2],dM,1)
lmiterm([8 1 3 X12],m1*Vp2,Vp2'*A20')
lmiterm([8 1 3 X22],m1*Vpp2,Vpp2'*A20')
lmiterm([8 1 3 -Z2],m1*C2',D20')
lmiterm([8 1 4 X12],m2*Vp2,Vp2'*E2bar')
lmiterm([8 1 4 X22],m2*Vpp2,Vpp2'*E2bar')
lmiterm([8 1 4 -Z2],m2*C2',G2bar')
lmiterm([8 1 6 X12],gama*m2*Vp2,Vp2'*H2')
lmiterm([8 1 6 X22],gama*m2*Vpp2,Vpp2'*H2')
lmiterm([8 2 2 T2],-1,1)
lmiterm([8 2 3 X12],m1*Vp2,Vp2'*B20')
lmiterm([8 2 3 X22],m1*Vpp2,Vpp2'*B20')
lmiterm([8 2 5 X12],m2*Vp1,Vp1'*F2bar')
lmiterm([8 2 5 X22],m2*Vpp1,Vpp1'*F2bar')
lmiterm([8 3 3 X11],-Vp1,Vp1')
lmiterm([8 3 3 X21],-Vpp1,Vpp1')
lmiterm([8 4 4 X11],-Vp1,Vp1')
lmiterm([8 4 4 X21],-Vpp1,Vpp1')
lmiterm([8 5 5 X11],-Vp1,Vp1')
lmiterm([8 5 5 X21],-Vpp1,Vpp1')
lmiterm([8 6 6 0],-1)

lmiterm([9 1 1 X11],-Vp1,Vp1')
lmiterm([9 1 1 X21],-Vpp1,Vpp1')
lmiterm([9 1 1 T1],dM,1)
lmiterm([9 1 3 X11],m1*Vp1,Vp1'*A10')
lmiterm([9 1 3 X21],m1*Vpp1,Vpp1'*A10')
lmiterm([9 1 3 -Z1],m1*C1',D10')
lmiterm([9 1 4 X11],m2*Vp1,Vp1'*E1bar')
lmiterm([9 1 4 X21],m2*Vpp1,Vpp1'*E1bar')
lmiterm([9 1 4 -Z1],m2*C1',G1bar')
lmiterm([9 1 6 X11],gama*m2*Vp1,Vp1'*H1')
lmiterm([9 1 6 X21],gama*m2*Vpp1,Vpp1'*H1')
lmiterm([9 2 2 T1],-1,1)
lmiterm([9 2 3 X11],m1*Vp1,Vp1'*B10')
lmiterm([9 2 3 X21],m1*Vpp1,Vpp1'*B10')
lmiterm([9 2 5 X11],m2*Vp1,Vp1'*F1bar')
lmiterm([9 2 5 X21],m2*Vpp1,Vpp1'*F1bar')
lmiterm([9 3 3 X11],-Vp1,Vp1')
lmiterm([9 3 3 X21],-Vpp1,Vpp1')
lmiterm([9 4 4 X11],-Vp1,Vp1')
lmiterm([9 4 4 X21],-Vpp1,Vpp1')
lmiterm([9 5 5 X11],-Vp1,Vp1')
lmiterm([9 5 5 X21],-Vpp1,Vpp1')
lmiterm([9 6 6 0],-1)

lmiterm([10 1 1 X12],-Vp1,Vp1')
lmiterm([10 1 1 X22],-Vpp1,Vpp1')
lmiterm([10 1 1 T2],dM,1)
lmiterm([10 1 3 X12],m1*Vp2,Vp2'*A20')
lmiterm([10 1 3 X22],m1*Vpp2,Vpp2'*A20')
lmiterm([10 1 3 -Z2],m1*C2',D20')
lmiterm([10 1 4 X12],m2*Vp2,Vp2'*E2bar')
lmiterm([10 1 4 X22],m2*Vpp2,Vpp2'*E2bar')
lmiterm([10 1 4 -Z2],m2*C2',G2bar')
lmiterm([10 1 6 X12],gama*m2*Vp2,Vp2'*H2')
lmiterm([10 1 6 X22],gama*m2*Vpp2,Vpp2'*H2')
lmiterm([10 2 2 T2],-1,1)
lmiterm([10 2 3 X12],m1*Vp2,Vp2'*B20')
lmiterm([10 2 3 X22],m1*Vpp2,Vpp2'*B20')
lmiterm([10 2 5 X12],m2*Vp2,Vp2'*F2bar')
lmiterm([10 2 5 X22],m2*Vpp2,Vpp2'*F2bar')
lmiterm([10 3 3 X12],-Vp2,Vp2')
lmiterm([10 3 3 X22],-Vpp2,Vpp2')
lmiterm([10 4 4 X12],-Vp2,Vp2')
lmiterm([10 4 4 X22],-Vpp2,Vpp2')
lmiterm([10 5 5 X12],-Vp2,Vp2')
lmiterm([10 5 5 X22],-Vpp2,Vpp2')
lmiterm([10 6 6 0],-1)

lmiterm([-11 1 1 X11],Vp1,Vp1')
lmiterm([-11 1 1 X21],Vpp1,Vpp1')
lmiterm([-11 1 1 0],-alpha)

lmiterm([-12 1 1 X12],Vp2,Vp2')
lmiterm([-12 1 1 X22],Vpp2,Vpp2')
lmiterm([-12 1 1 0],-alpha)

lmis = getlmis;
[tmin,xfeas] = feasp(lmis);
%X1 = dec2mat(lmis,xfeas,X1);
%X2 = dec2mat(lmis,xfeas,X2);
X11 = dec2mat(lmis,xfeas,X11);
X21 = dec2mat(lmis,xfeas,X21);
X12 = dec2mat(lmis,xfeas,X12);
X22 = dec2mat(lmis,xfeas,X22);
T1 = dec2mat(lmis,xfeas,T1);
T2 = dec2mat(lmis,xfeas,T2);
Z1 = dec2mat(lmis,xfeas,Z1);
Z2 = dec2mat(lmis,xfeas,Z2);

% X11=V1'*X1*V1;X11=X11(1,1);
% X22=V2'*X2*V2;X22=X22(1,1);

X1=Vp1*X11*Vp1'+Vpp1*X21*Vpp1';
X2=Vp2*X12*Vp2'+Vpp2*X22*Vpp2';

K1=Z1*U1*C10*inv(X11)*inv(C10)*U1';
K2=Z2*U2*C20*inv(X12)*inv(C20)*U2';

toc

%Initial State and Parameters

for i=1:6
    xsb(:,i)=[0.5;-0.5];
    nsb(i) =norm(xsb(:,i));
end

T0=dM+2;Tf=T0+40;

%Simulation
for i=T0:Tf
    d=[2*ones((Tf-T0)/4,1);3*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1);2*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1)];
    dp1=e1*sin(i);
    z=mod(i,2);
    if (z>0)
        xsb(:,i) =((A10+dp1*E1)+(D10+dp1*G1)*K1*C1)*xsb(:,i-1)+(B10+dp1*F1)*xsb(:,i-1-d(i))+[0.01*sin(xsb(1,i-1));0];
        ysb(:,i-1)=C1*xsb(:,i-1);
        usb(:,i-1)=K1*ysb(:,i-1);
        nsb(i) =norm(xsb(:,i));
        Vsb(i-1)=xsb(:,i-1)'*inv(X1)*xsb(:,i-1);
        for k=-1:-1:-dM
            Vsb(i-1)=Vsb(i-1)+xsb(:,i-1+k)'*inv(X1)*T1*inv(X1)*xsb(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            Vsb(i-1)=Vsb(i-1)+(dM+k)*xsb(:,i-1+k)'*inv(X1)*T1*inv(X1)*xsb(:,i-1+k);
        end
        Vsb(i-1);
    else
        xsb(:,i)=((A20+dp1*E2)+(D20+dp1*G2)*K2*C2)*xsb(:,i-1)+(B20+dp1*F2)*xsb(:,i-1-d(i))+[0.01*sin(xsb(1,i-1));0];
        ysb(:,i-1)=C2*xsb(:,i-1);
        usb(:,i-1)=K2*ysb(:,i-1);
        nsb(i) =norm(xsb(:,i));
        Vsb(i-1)=xsb(:,i-1)'*inv(X2)*xsb(:,i-1);
        for k=-1:-1:-dM
            Vsb(i-1)=Vsb(i-1)+xsb(:,i-1+k)'*inv(X2)*T2*inv(X2)*xsb(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            Vsb(i-1)=Vsb(i-1)+(dM+k)*xsb(:,i-1+k)'*inv(X2)*T2*inv(X2)*xsb(:,i-1+k);
        end
        Vsb(i-1);
    end
    Jsb=Jsb+ysb(:,i-1)'*R1*ysb(:,i-1)+usb(:,i-1)'*R2*usb(:,i-1);
end

save('SOF.mat')

CostFunction=Jsb;

usb1max=max(usb(1,T0-1:Tf-1));
usb1min=min(usb(1,T0-1:Tf-1));
usb1Max=max(abs(usb1min),abs(usb1max));

ysb1max=max(ysb(1,T0:Tf-1));
ysb1min=min(ysb(1,T0:Tf-1));
ysb1Max=max(abs(ysb1min),abs(ysb1max));

List={};
Result_table = table(dM,CostFunction,usb1Max,ysb1Max,'RowNames',List)

t=0:Tf-T0;
stairs(t,xsb(:,T0-1:Tf-1)','-','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('States');
figure;stairs(t,usb(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Control Signal (u)');
figure;stairs(t,ysb(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Output (y)');
%figure;stairs(t,Vsb(T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('V');
%figure;plot(t,nsb(T0:Tf)','*');grid on;xlabel('k (sample)');ylabel('Norm of state variables');